/*
此代码用于根据pointcloud2点云进行octomap二维建图，不用指定tf关系，但是只能显示二维效果
*/
#include "ros/ros.h"
#include "sensor_msgs/PointCloud2.h"
 
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
 
#include <octomap/octomap.h>
#include <octomap/Pointcloud.h>
#include <octomap/ColorOcTree.h>
#include <octomap_msgs/Octomap.h>
 
#include "myoctomap/pcl_conversions.h"

#include "/home/hkb/ros_source/octomap_msgs/include/octomap_msgs/conversions.h"
 
 
// Create an Octomap message
octomap_msgs::Octomap octomap_msg;
 
class MapGenerator{
public:
   MapGenerator(){
      tree = new octomap::OcTree(0.1);
   };
   ~MapGenerator(){};
 
   void PointCloudCallback(const sensor_msgs::PointCloud2::ConstPtr& msg);
 
   octomap::OcTree* tree;
};
 
void print_query_info(octomap::point3d query, octomap::OcTreeNode* node) {
   if (node != NULL) {
      std::cout << "occupancy probability at " << query << ":\t " << node->getOccupancy() << std::endl;
   }
   else 
      std::cout << "occupancy probability at " << query << ":\t is unknown" << std::endl;    
}
 
 
int main(int argc, char **argv)
{
    // 初始化ROS节点
    ros::init(argc, argv, "pointcloud_subscriber");
    // 创建ROS节点句柄
    ros::NodeHandle nh;
 
    MapGenerator mg;
 
    // 创建Subscriber来订阅PointCloud2消息
    ros::Subscriber sub = nh.subscribe<sensor_msgs::PointCloud2>("/robot1/pandar", 10, boost::bind(&MapGenerator::PointCloudCallback, &mg, _1));
 
    // Create a publisher for the Octomap topic
    // ros::Publisher octomap_pub = nh.advertise<octomap_msgs::Octomap>("octomap_tree", 1);
    ros::Publisher octomap_pub = nh.advertise<octomap_msgs::Octomap>("projected_map", 1);
    // Publish the Octomap
    ros::Rate loop_rate(1);  // Adjust the publishing rate as needed
    while (ros::ok()) {
      octomap_pub.publish(octomap_msg);
      ros::spinOnce();
      loop_rate.sleep();
    }
 
    return 0;
}
 
// 回调函数，用于处理接收到的PointCloud2消息
void MapGenerator::PointCloudCallback(const sensor_msgs::PointCloud2::ConstPtr& msg)
{
   pcl::PointCloud<pcl::PointXYZ> pcl_cloud;
   pcl::fromROSMsg(*msg, pcl_cloud);
 
   for (const auto& point : pcl_cloud) {
      // Extract XYZ coordinates and RGB color from the point
      float x = point.x;
      float y = point.y;
      float z = point.z;
 
      // Convert color to octomap::ColorOcTreeNode
      octomap::point3d octomap_point(x, y, z);
 
      // Insert the node into the octomap
      tree->updateNode(octomap_point, true);
   }
 
   // octomap::point3d query = octomap::point3d(1., 2., 0.);
   // octomap::OcTreeNode* result = tree->search(query);
   // print_query_info(query, result);
 
   
   // Also publish as an octomap msg for visualization
   octomap_msgs::fullMapToMsg(*tree, octomap_msg);
   
//    在这里可以对OctoMap进行其他操作，如保存地图数据
//    tree.writeBinary("your_octomap.ot");
 
   ROS_INFO("OctoMap created and saved.");
}
 